/**
  ******************************************************************************
  * File Name          : rocr6_moveit_driver.cpp
  * Description       : rocr6_moveit_driver program body
  ******************************************************************************
  *
  * Copyright (c) 2019 HopeMotion Co., Ltd. 
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without 
  * modification, are permitted, provided that the following conditions are met:
  *
  * 1. Redistribution of source code must retain the above copyright notice, 
  *    this list of conditions and the following disclaimer.
  * 2. Redistributions in binary form must reproduce the above copyright notice,
  *    this list of conditions and the following disclaimer in the documentation
  *    and/or other materials provided with the distribution.
  * 3. Neither the name of HopeMotion nor the names of other 
  *    contributors to this software may be used to endorse or promote products 
  *    derived from this software without specific written permission.
  * 4. This software, including modifications and/or derivative works of this 
  *    software, must execute solely and exclusively on microcontroller or
  *    microprocessor devices manufactured by or for HopeMotion.
  * 5. Redistribution and use of this software other than as permitted under 
  *    this license is void and will automatically terminate your rights under 
  *    this license. 
  *
  * THIS SOFTWARE IS PROVIDED BY HOPEMOTION AND CONTRIBUTORS "AS IS" 
  * AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT 
  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 
  * PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
  * RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT 
  * SHALL HOPEMOTION OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 
  * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 
  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 
  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  ******************************************************************************
  */
/* Includes ------------------------------------------------------------------*/
#include <rocr6_moveit_driver/rocr6_moveit_driver.h>

//double joint_position[JOINT_MAX] = {0,0,0,0,0,0};

std::string _port;

#define ROCR6_IDLE    0
#define ROCR6_PLAY    1
#define ROCR6_STOP    2

double rx_data[JOINT_MAX] = {0, 0, 0, 0, 0, 0};
double tx_data[JOINT_MAX] = {0, 0, 0, 0, 0, 0};

int last_state = ROCR6_IDLE;
int rocr6_run = ROCR6_IDLE;
/* Private function prototypes -----------------------------------------------*/

/**
  * @brief  机械臂定义
  * @param  
  * @retval 无
  */
class HwDriver
{
  protected:
    ros::NodeHandle nh;
    std::string action_name;
    //定义action服务端
    actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>  as;    
    //定义action服务端目标控制句柄
    actionlib::ServerGoalHandle<control_msgs::FollowJointTrajectoryAction> goal_handle; 
    //反馈action目标的执行情况，客户端由此可以得知服务端是否执行成功了
    control_msgs::FollowJointTrajectoryResult result;

  public:
    HwDriver(std::string name) :
      as(nh, name , boost::bind(&HwDriver::goalCB, this, _1), false), action_name(name)
    {
      as.start();
    }

    ~HwDriver(void)
    {
    }

    void goalCB(actionlib::ServerGoalHandle<control_msgs::FollowJointTrajectoryAction> gh)
    {
      actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = *gh.getGoal();
      control_msgs::FollowJointTrajectoryGoal::_trajectory_type trajectory;
	    trajectory_msgs::JointTrajectory::_points_type::iterator iter;

      //获取运动轨迹
      trajectory = goal.trajectory;
      //printf("trajectory: \r\n");
	    // for(iter= trajectory.points.begin(); iter!=trajectory.points.end(); iter++) 
      // {
      //   printf(" %f %f %f %f %f %f \r\n" ,
      //                   iter->positions[0], iter->positions[1], iter->positions[2], iter->positions[3], iter->positions[4], iter->positions[5]);
      // }
      iter = (trajectory.points.end())-1;
      printf("Goal: %f %f %f %f %f %f \r\n" ,
                      iter->positions[0], iter->positions[1], iter->positions[2], iter->positions[3], iter->positions[4], iter->positions[5]);

      // std::string ns[JOINT_MAX] ;
      // printf("ns:[");
      // for(int j=0; j<JOINT_MAX; j++)
      // {
      //   ns[j]= trajectory.joint_names[j];
      //   printf(" %s", ns[j].c_str());
      // }
      // printf(" ]\r\n");

      //控制机械臂
      tx_data[0] = rocr6_tf_tdata(iter->positions[2]);
      tx_data[1] = rocr6_tf_tdata(iter->positions[1]);
      tx_data[2] = rocr6_tf_tdata(iter->positions[0]);
      tx_data[3] = rocr6_tf_tdata(iter->positions[3]);
      tx_data[4] = rocr6_tf_tdata(iter->positions[4]);
      tx_data[5] = rocr6_tf_tdata(iter->positions[5]);
 
      rocr6_txSig(1, 0, tx_data); 
      
      //运行结束返回
      goal_handle = gh;  
      goal_handle.setAccepted();    
      result.error_code = result.SUCCESSFUL;
      goal_handle.setSucceeded(result);
    }
};

/**
  * @brief  机械臂驱动主函数
  * @param  消息结构体指针
  * @retval 无
  */
int main(int argc, char** argv)
{
  ros::init(argc, argv, "rocr6_moveit_driver");
  ros::NodeHandle nh;
  ros::Rate loop_rate(50);

  //发布机械臂状态
  ros::Publisher pub_states = nh.advertise<sensor_msgs::JointState>("joint_states", 100);

  sensor_msgs::JointState joint_state;

  nh.getParam("/rocr6_moveit_driver/joint_names", joint_state.name);
  nh.getParam("/rocr6_moveit_driver/joint_position", joint_state.position);
  nh.getParam("/rocr6_moveit_driver/port", _port);

  // for(int i=0; i< JOINT_MAX; i++)
  // {
  //    printf("%s\r\n", joint_state.name[i].c_str());
  // }

  HwDriver rocr6("rocr6/arm_joint_controller/follow_joint_trajectory");

  if(rocr6_init(_port.c_str()))
  {
    rocr6_brake(OPT_ON);
    usleep(6000000);
    rocr6_broadcast(OPT_ON);
  }
  else 
  {
    exit(1);
  }

  while(ros::ok())
  {
    rocr6_rxSig(rx_data);
    joint_state.header.stamp = ros::Time::now();
    for(int i=0; i<JOINT_MAX; i++)
    {
      joint_state.position[i] =rocr6_tf_rdata(rx_data[i]);  
    }
    pub_states.publish(joint_state);

    ros::spinOnce();
    loop_rate.sleep();
  }

  rocr6_broadcast(OPT_OFF);
  //rocr6_brake(OPT_OFF);
  rocr6_deinit();

  return 0;
}
